function [x_opt,a_opt] = optimization_NAO_closed(LineNumber)
global ns_time s_time ns_slope s_knee ns_knee hip_pos pos_com q0 Hminus


OutPutType = num2str(LineNumber);
load('data/IndexAll');

indexHippos = IndexAll(LineNumber,1);
indexSlope = IndexAll(LineNumber,2);
indexTorso = IndexAll(LineNumber,3);

%%%%%%%%%%%%%Add paths to mathematica compiled files:
addpath('./build_torso')
addpath('./wrappers_torso')
addpath('./data')



%%%%%%%%%%%%%Loading the data:
load('mean_data_hippos.mat') % 1
load('mean_data_LinearHippos.mat') 
load('mean_data_LinearHipposD.mat') 
load('mean_data_nsslope.mat')
load('mean_data_hipAngle.mat')
load('mean_data_LinearNSslope.mat')
load('mean_data_sknee.mat')
load('mean_data_nsknee.mat')
load('mean_data_nstorso.mat')
load('mean_data_LinearNStorso.mat')
load('mean_data_storso.mat')
load('mean_data_LinearStorso.mat')
load('mean_data_theta4.mat')
load('mean_data_TorsoHipAngle.mat')
%%%%%%%%%%%%%Put desired torso data here
switch indexHippos
    case 1
        hip_pos = hip_pos;
    case 2
        hip_pos = Linear_hippos;
    case 3
        hip_pos = Linear_hippos_D;
end
    
switch indexSlope
    case 1
        ns_slope = ns_slope;
    case 2
        ns_slope = Linear_nsslope;
    case 3
        ns_slope = hip_angle;
end

switch indexTorso
    case 1
        pos_com = torso_angle;
    case 2
        pos_com = theta4;
    case 3
        pos_com = ns_torso;
    case 4
        pos_com = Linear_ns_torso;
    case 5 
        pos_com = s_torso;
    case 6
        pos_com = Linear_s_torso;
end

s_time = ns_time;

%%%%%%%%%%%%%Outputs from loading the data:
% HipData = [ns_time, hip_pos];
% NSslopeData = [ns_time,ns_slope];
% SkneeData = [s_time, s_knee];
% NSkneeData = [ns_time,ns_knee];
% PcomData = [ns_time, pos_com];




%%%%%%%%%%%%%Initial fit parameters (as determined by mathemaitca right now):
%%%%%%%%%%%%%% TODO:  determine the initial parameters from the data in
%%%%%%%%%%%%%% matlab


load('Afit.mat', 'a0');
% a0

%%%%%%%%%%%%%% Minimization to verify optimality of initial parameters w.r.t the HBC
% options = optimset ('Display','iter','MaxFunEvals',6000);
% [a_fit,cost_fit] = fminunc(@cost,flatten(a0),options)


%%%%%%%%%%%%%% In the case that you don't want to run the optimizatino to
%%%%%%%%%%%%%% verify the parameters, uncomment this:
 a_fit = flatten(a0);

%%%%%%%%%%%%%% Constrain the parameters of the hip position to be zero
ubmat = Inf*ones(5,5);
lbmat = -Inf*ones(5,5);
ubmat(1,2) = 0; ubmat(1,3) = 0; ubmat(1,4) = 0; ubmat(1,5) = 0; 
lbmat(1,2) = 0; lbmat(1,3) = 0; lbmat(1,4) = 0; lbmat(1,5) = 0;
ubmat(1,1) = 0.8;%0.7% 1 %1.5%0.6
lbmat(1,1) = 0.1;
% %   9 
% ubmat(5,1) = 0; ubmat(5,2) = 0; ubmat(5,3) = 0; ubmat(5,4) = 0; 
% lbmat(5,1) = 0; lbmat(5,2) = 0; lbmat(5,3) = 0; ubmat(5,4) = 0;
% lbmat(1,1) = 0.1;
ub = flatten(ubmat);
lb = flatten(lbmat);




%%%%%%%%%%%%%% Other conditions for the optimization:
Aeq = [];
beq = [];
A = [];
b = [];

%%%%%%%%%%%%%% Optimizing to achieve HZD with the HBC:

%%%%% Conditions for optimization: tight error 
 errorbound = 1e-5;
options = optimset('Algorithm','sqp',...
    'Display','iter','TolX',errorbound,...
'MaxFunEvals',20000,'TolCon',errorbound);

%%%%% Conditions for optimization:
% options = optimset ('Display','iter','MaxFunEvals',6000);

%%%%% Optimization
tic
[a_out,cost_out] = fmincon(@cost,a_fit,A,b,Aeq,beq,lb,ub,@mycon,options);
toc



%%%%%%%%%%%%%% Output the parameter matrix and the fit matrix:
a_opt = unflatten(a_out);
a_fit = unflatten(a_fit);


%%%%%%%%% criterion %%%%%%%%%%%%%
NAO_hippos = yd_hip_pos(ns_time,a_opt);
R_hippos = corrcoef(hip_pos,NAO_hippos(:));  % hip_position

NAO_nsslope = yd_ns_slope(ns_time,a_opt);
R_nsslope = corrcoef(ns_slope,NAO_nsslope(:));% non-stance slope

NAO_sknee = yd_s_knee(ns_time,a_opt);
R_sknee = corrcoef(s_knee, NAO_sknee);% stance knee

NAO_nsknee = yd_ns_knee(ns_time,a_opt);
R_nsknee = corrcoef(ns_knee, NAO_nsknee);% non-stance knee

NAO_torso = yd_pos_com(ns_time,a_opt);
R_torso = corrcoef(pos_com, NAO_torso);% torso angle

R_data = [R_hippos(2,1);R_nsslope(2,1);R_sknee(2,1);R_nsknee(2,1);R_torso(2,1)];

% save('R_data_TorsoAngle.mat','R_data')
% save('R_data_NSTorsoSlope.mat','R_data')
% save('R_data_STorsoSlope.mat','R_data')
% save('R_data_NSCOMSlope.mat','R_data')
Rname = strcat('result/R_data_',OutPutType,'S.mat');
save(Rname,'R_data')
%%%%%%%%%%%%%% Output the initial condition:
q_opt = inverse_kin(a_opt);
% qdot_opt = H_minus(q_opt,a_opt)\[a_opt(1,1); 0; 0; 0; 0];  %%%%%%% SHU

qdot_opt = Hminus\[a_opt(1,1); 0; 0; 0; 0];
x_opt = [q_opt; qdot_opt];

%%%%%%%%%%%%%% Output the final time: 
% disp(['Step time: ' tau(q_opt,a_opt)]);


%%%%%%%%%%%%%% Save initial and final condition
save('result/x_opt.mat','x_opt')
save('result/a_opt.mat','a_opt')

%%%%%%%%%%%%%% Plot the output vs. fit vs. the human data

fig12 = figure(12); clf; 
subplot(2,3,1); plot(ns_time,yd_hip_pos(ns_time,a_opt),'b',ns_time,yd_hip_pos(ns_time,a_fit),'r',ns_time,hip_pos,'k.') 
title('Hip Position')
xlabel('time (s)');
% ylabel('Desired vs. Fit vs. Data')
legend({'Desired','Fit','Data'});
subplot(2,3,2); plot(ns_time,yd_ns_slope(ns_time,a_opt),'b',ns_time,yd_ns_slope(ns_time,a_fit),'r',ns_time,ns_slope,'k.')
title('Non-Stance Slope')
xlabel('time (s)');
% ylabel('Desired vs. Fit vs. Data')
legend({'Desired','Fit','Data'});
subplot(2,3,4); plot(s_time,yd_s_knee(s_time,a_opt),'b',s_time,yd_s_knee(s_time,a_fit),'r',s_time,s_knee,'k.')
title('Stance Knee')
xlabel('time (s)');
% ylabel('Desired vs. Fit vs. Data')
legend({'Desired','Fit','Data'});
subplot(2,3,5); plot(ns_time,yd_ns_knee(ns_time,a_opt),'b',ns_time,yd_ns_knee(ns_time,a_fit),'r',ns_time,ns_knee,'k.')
title('Non-Stance Knee')
xlabel('time (s)');
% ylabel('Desired vs. Fit vs. Data')
legend({'Desired','Fit','Data'});
subplot(2,3,3); plot(ns_time,yd_pos_com(ns_time,a_opt),'b',ns_time,yd_pos_com(ns_time,a_fit),'r',ns_time,pos_com,'k.')
title('Torso')
xlabel('time (s)');
% ylabel('Desired vs. Fit vs. Data')
legend({'Desired','Fit','Data'});

% save('COMSlope.pdf',fig12)
% % saveas('TorosNSSlope.pdf',fig12)
% saveas(fig12,'TorosSSlope.pdf')
figname = strcat('result/',OutPutType,'S.pdf');
% saveas(fig12,'LNST4.pdf')
saveas(fig12,figname)
%%%%%%%%%%%%%% Run main or step
main_NAO(OutPutType)
close all;
end



function ret = cost(aflat)
global ns_time s_time ns_slope s_knee ns_knee hip_pos pos_com


% % % scaled time 
% % % step length of fred 0.6173
% % ns_time = (0.6713/aflat(1))*ns_time/ns_time(end);
% % s_time = ns_time;
% % ns_time(end)


%%%%%%%%%%%%%% Unflatten the a matrix
a = unflatten(aflat);

%%%%%%%%%%%%%% Cost for each canonical function
% cost_ns_slope = norm(ns_slope-yd_ns_slope(ns_time,a))^2;
% cost_hip_pos = norm(hip_pos-yd_hip_pos(ns_time,a))^2;
% cost_s_knee = norm(s_knee-yd_s_knee(s_time,a))^2;
% cost_ns_knee = norm(ns_knee-yd_ns_knee(ns_time,a))^2;
% cost_pos_com = norm(pos_com-yd_pos_com(ns_time,a))^2;

%%%%%%%%%%%%%% Cost for each canonical function

cost_ns_slope = sum(abs(ns_slope-yd_ns_slope(ns_time,a)));
cost_hip_pos = sum(abs(hip_pos-yd_hip_pos(ns_time,a)));
cost_s_knee = sum(abs(s_knee-yd_s_knee(s_time,a)));
cost_ns_knee = sum(abs(ns_knee-yd_ns_knee(ns_time,a)));
cost_pos_com = sum(abs(pos_com-yd_pos_com(ns_time,a)));

%%%%%%%%%%%%%% Wieghting for the fits

beta_ns_slope = 1/(max(ns_slope)-min(ns_slope));
beta_hip_pos = 1/(max(hip_pos)-min(hip_pos));
beta_s_knee = 1/(max(s_knee)-min(s_knee));
beta_ns_knee = 1/(max(ns_knee)-min(ns_knee));
beta_pos_com = 1/(max(pos_com)-min(pos_com));
beta = beta_ns_slope+beta_hip_pos+beta_s_knee+beta_ns_knee+beta_pos_com;

cost_all = [
      beta_ns_slope*cost_ns_slope,...
      beta_hip_pos*cost_hip_pos,...
      beta_s_knee*cost_s_knee,...
      beta_ns_knee*cost_ns_knee,...
      beta_pos_com*cost_pos_com
      ];


  
%%%%%%%%%%%%%% Return the cost
 ret = sum(cost_all)/beta;

end


%%%%%%%%%%%%%% Constraints to enforce HZD (or partial HZD)

function [c,ceq] = mycon(aflat)
global ns_time Hminus


%%%%%%%%%%%%%% Unflatten the a matrix
a = unflatten(aflat);

%%%%%%%%%%%%%% Determine the initial condition (on the guard) for the
%%%%%%%%%%%%%% current parameters

%%

%%%%%%%%%%Determine the initial position on the guard


q_minus = inverse_kin(a);

%%

%%%%%%%%%%Determine the pre and post impact values of the output functions

%%%%Initial position of the hip
dimq = length(q_minus);
q_plus = R(q_minus);
%%%%%%%% dot phip_sca %%%%%%%%%%%%%%%%%%%%% TO DO
%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
phipcond = phip_sca(q_plus);

%%%%Pre-impact relative degree 2 outputs
yminus = ya2_vec(q_minus, phipcond, a)-yd2_vec(q_minus, phipcond,  a);
% y2plus = ya2_vec(q_plus, phipcond, a)-yd2_vec(q_plus, phipcond,  a)

%%%%Pre-and post impact relative Jacobians of the outputs

Dy2_plus = Dya2_mat(q_plus, phipcond, a)-Dyd2_mat(q_plus, phipcond,  a);
Hplus = [ phip_dot_mat(q_plus);
          Dy2_plus(:,1:dimq)];
       
Dy2_minus = Dya2_mat(q_minus, phipcond, a)-Dyd2_mat(q_minus, phipcond,  a);
Hminus = [ phip_dot_mat(q_minus);
          Dy2_minus(:,1:dimq)];
% disp(Dy2_minus)
% disp(phip_dot_mat(q_minus))

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%% The Time to impact (assuming no scuffing)
Ttau = sigma_sca(q_minus,phipcond,a);
if imag(Ttau)>0||Ttau < 0
Ttau = 1;
end

Tmin = fminbnd(@(t)height(t,a,phipcond),0,Ttau);%%%%%%%%%%%%%% 0801  %%%%%%%%%%
hmin = height(Tmin,a,phipcond);%%%%%%%%%%%%%% 0801  %%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%Pre- and Post-impact velocities


qdot_minus = Hminus\[a(1,1); 0; 0; 0; 0];
x_minus = [q_minus; qdot_minus];

qdot_plus = P(x_minus);
x_plus = [q_plus; qdot_plus];


H = Hplus*P(x_minus);

%%


    %%%%%%%%%%%%%% Require the post impact velocities be completlty determined:
       Hzd = H - [a(1,1); 0; 0; 0; 0];
%      Hzd = H(2:end,:) - [0; 0; 0; 0]; %%%%%%%%%%%%%% 0801  %%%%%%%%%%

    %%%%%%%%%%%%%% Pre and Post Impact knee angles 
 
    %%%%%%%%%%%%%% Nonlinear inequalities
    %%%%%%%%%%%%%%%%% gaurd must be negative
    %%%%%%%%%%%%%%%%% Time must be less than the human time
    c = [
           h_dot_mat(q_minus)*qdot_minus+.01;
%            -hmin;
%             -hmin - .001; %%%%%%%% 0913
        ];
    
%%%%%%%%%%%%%% Nonlinear equalities
%%%%%%%%%%%%%%%%% y pre impact must be zero
%%%%%%%%%%%%%%%%% y dot post impact must be zero
%%%%%%%%%%%%%%%%% guard must be satisfied
ceq = [
       yminus;
       Ttau-ns_time(end);  %%%%
       Hzd];

   
end

%%
%%%%%%%Computing the position at impact through inverse kinematics

function ret = inverse_kin(a)
% global q0

% 

typeofsolve = 1;


if typeofsolve == 1
    
    %%%%%%%%%%%%%%%%%%%%% Numeric inverse kinematics
    %%%%%q0 as computed from the human parameters
    load('ic_hm.mat')  %%%%%%%%%%% SHU
    ic = x_opt;
%      load('x_opt.mat')
%      ic = x_opt;
%     q0 = [ -0.5138
%         0.2776
%         0.2870
%         -0.4201
%         0.1746];
    q0 = ic(1:5,:);
    %%%Numerically solving for the initial condition
    
    options = optimset('Display','off');
    qzero = fsolve(@(q)inverse_kin_fun(q,a),q0,options);
    % %%%%If you update the inital guess on each iteration, the optimization does
    % %%%%not converge
    % q0 = qzero
    ret = qzero;
    
elseif typeofsolve == 2
    %%%%If using the old version (no torso) use this:
    
    % theta_a(a)
    %
    
    
    %%%If using the new version with the torso, use this:
    %%%%(the calculation is broken up, saves a lot of computation time)
    
    q = zeros(5,1);
    q(2) = theta_a2(q,a);
    q(5) = theta_a5(q,a);
    q(1) = theta_a1(q,a);
    q(4) = theta_a4(q,a);
    q(3) = theta_a3(q,a);
%      q(4) = theta_a4(q,a);
    
    ret = q;
    
end


end

function F = inverse_kin_fun(q,a)

phipcond = phip_sca(R(q));
y2plus = ya2_vec(R(q), phipcond, a)-yd2_vec(R(q), phipcond,  a);

F = [y2plus;
     h_sca(q)];

% F = [y_plus(q,a);
%      h_sca(q)];

end

% function ret = y_plus(q,a)
% 
% phipcond = phip_sca(R(q));
% y2plus = ya2_vec(R(q), phipcond, a)-yd2_vec(R(q), phipcond,  a);
% 
% ret = y2plus;
% 
% end



%%
% 
% %%%%%%%%%Computing the constraint functions that ensure hybrid zero
% %%%%%%%%%dynamics
% 
% function ret = tau(q,a)
% 
% 
% phipcond = phip_sca(R(q));
% ret = sigma_sca(q,phipcond,a);
% 
% end
% 
% function ret = y_minus(q,a)
% 
% 
% phipcond = phip_sca(R(q));
% y2minus = ya2_vec(q, phipcond, a)-yd2_vec(q, phipcond,  a);
% 
% ret = y2minus;
% 
% end
% 
% function ret = H_plus(q,a)
% 
% 
% dimq = length(q);
% phipcond = phip_sca(R(q));
% Dy2 = Dya2_mat(R(q), phipcond, a)-Dyd2_mat(R(q), phipcond,  a);
% Hplus = [ phip_dot_mat(R(q));
%            Dy2(:,1:dimq)];
% 
% ret = Hplus;
% 
% end
% 
% function ret = H_minus(q,a)
% 
% 
% dimq = length(q);
% phipcond = phip_sca(R(q));
% Dy2 = Dya2_mat(q, phipcond, a)-Dyd2_mat(q, phipcond,  a);
% Hplus = [ phip_dot_mat(q);
%            Dy2(:,1:dimq)];
% 
% ret = Hplus;
% 
% end
% 




%%

%%%%%%%%%%%%%% Computing the position and velocity components of the impact
%%%%%%%%%%%%%% equations

function ret = R(q)

dimq = length(q);
x = [q; zeros(dimq,1)];
xplus = resetFunc(x);

ret = xplus(1:dimq);    
        
end   


function ret = P(x)
      
dimq = length(x)/2;
xplus = resetFunc(x);

ret = xplus(dimq+1:end);    
        
end     


%%

%%%%%%%%%%%%%% Canonical human functions

function ret = yd_ns_slope(t,a)

ret =exp(-a(2,4)*t).*(a(2,1)*cos(a(2,2)*t)+a(2,3)*sin(a(2,2)*t))+a(2,5);

end

function ret = yd_hip_pos(t,a)

ret = a(1,1)*t;

end

function ret = yd_s_knee(t,a)

ret = exp(-a(3,4)*t).*(a(3,1)*cos(a(3,2)*t)+a(3,3)*sin(a(3,2)*t))+a(3,5);

end

function ret = yd_ns_knee(t,a)

ret = exp(-a(4,4)*t).*(a(4,1)*cos(a(4,2)*t)+a(4,3)*sin(a(4,2)*t))+a(4,5);

end

function ret = yd_pos_com(t,a)
%%%% 6. linear torso 
ret = exp(-a(5,4)*t).*(a(5,1)*cos(a(5,2)*t)+a(5,3)*sin(a(5,2)*t))+a(5,5);
% ret = a(5,1)*t+a(5,2);
end

function aflat = flatten(a)
aflat = a(:);

end

function aunflat = unflatten(aflat)
global q0

qdim = 5;
aunflat = reshape(aflat,qdim,5);

end

%%

%%%%%%%%%%%%%% Limiting the torque



function ret = u_calc(q, p, a, ep)


ndof = length(q)/2;
dx = q(ndof+1:end);
% q
% pause(1000)
M    = D_mat(q);
C    = C_mat(q);
G    = G_vec(q);

% pause(0.5);

%%%Outputs
y_a1     = ya1_sca(q, p, a);
y_a2     = ya2_vec(q, p, a);
y_d1     = yd1_sca(q, p,  a);
y_d2     = yd2_vec(q, p,  a);

%%%Outputs
y1 = y_a1 - y_d1;
y2 = y_a2 - y_d2;


y_a = [y_a1; y_a2];
y_d = [y_d1; y_d2];

%%%Jacobians of Outputs
Dy_a1    = Dya1_mat(q, p, a);
Dy_d1    = Dyd1_mat(q, p, a);
Dy_a2    = Dya2_mat(q, p, a);
Dy_d2    = Dyd2_mat(q, p, a);

Dy_1 = Dy_a1 - Dy_d1;
Dy_2 = Dy_a2 - Dy_d2;

%%%Control Fields
vf    = [dx; M \ (-C*dx - G)];
B_IO  = eye(ndof);
gf    = [zeros(size(B_IO)); M \ B_IO];

%%%Lie Derivatives
Lgy1 = Dy_1*gf;
Lgy2 = Dy_2*gf;
Lfy1 = Dy_1*vf;
Lfy2 = Dy_2*vf;

%%%Second Order Jacobians

DLfy_a2  = DLfya2_mat(q, p, a);
DLfy_d2  = DLfyd2_mat(q, p, a);

DLfy2 = DLfy_a2-DLfy_d2;

%%%Second Lie Derivatives

LfLfy2 = DLfy2*vf;
LgLfy2 = DLfy2*gf;

% tp = [tp, t+tp0];
% yact = [yact, y_a];
% ydes = [ydes, y_d];

% Vector field

A = [Lgy1; LgLfy2];

u = -A \ ([0; LfLfy2]+[Lfy1; 2*ep*Lfy2]+ [ep*y1; ep^2*y2]);


% U = [U, u];
% Pow = [Pow, u.*dx];

ret = u;

end

